/**
 * Copyright (c) 2019 LG Electronics, Inc.
 *
 * This software contains code licensed as described in LICENSE.
 *
 */

using System;
using System.Linq;
using System.Collections.Generic;
using UnityEngine;
using Simulator.Bridge.Data;

public class RadarMesh : MonoBehaviour
{
    public enum RadarRangeType { SHORT, FAR };
    public RadarRangeType Range;
    public bool DisplayHandles { get; set; } = false;
    public float RadarMeshHeight = 1f;
    public List<Vector3> LocalPositions = new List<Vector3>();
    public MeshRenderer RadarMeshRenderer { get; set; }

    Action<Collider, RadarMesh> triggerEnter;
    Action<Collider, RadarMesh> triggerStay;
    Action<Collider, RadarMesh> triggerExit;
    LayerMask mask;
    
    public void Init()
    {
        RadarMeshRenderer = GetComponent<MeshRenderer>();
        RefreshMesh();
        RadarMeshRenderer.enabled = false;
    }
    
    public void SetMeshVisible(bool state)
    {
        if (RadarMeshRenderer.enabled != state)
        {
            RadarMeshRenderer.enabled = state;
        }
    }

    public void SetCallbacks(Action<Collider, RadarMesh> stay, Action<Collider, RadarMesh> exit)
    {
        triggerStay = stay;
        triggerExit = exit;
        mask = LayerMask.GetMask("NPC", "Pedestrian", "Bicycle", "Agent");
    }
    
    void OnTriggerStay(Collider other)
    {
        if (triggerStay == null || ((mask.value >> other.gameObject.layer) & 1) == 0)
        {
            return;
        }
        
        triggerStay(other, this);
    }

    private void OnTriggerExit(Collider other)
    {
        if (triggerExit == null || ((mask.value >> other.gameObject.layer) & 1) == 0)
        {
            return;
        }
        
        triggerExit(other, this);
    }
    
    public void RefreshMesh()
    {
        // indices top and bottom
        Vector2[] temp = new Vector2[LocalPositions.Count];
        for (int i = 0; i < temp.Length; i++)
            temp[i] = new Vector2(LocalPositions[i].x, LocalPositions[i].z);
        
        int[] indices = new Triangulator(temp).Triangulate();
        int[] reverseIndices = indices.Reverse().ToArray();
        int[] tempAllIndices = new int[indices.Length * 2];

        reverseIndices.CopyTo(tempAllIndices, 0);
        indices.CopyTo(tempAllIndices, indices.Length);

        for (int i = indices.Length; i < tempAllIndices.Length; i++)
            tempAllIndices[i] += LocalPositions.Count;
        
        // indices sides
        List<int> tempIntList = tempAllIndices.ToList();
        for (int i = 0; i < LocalPositions.Count; i++)
        {
            List<int> t = new List<int>();
            for (int j = 0; j < 6; j++)
            {
                if (j == 0)
                    t.Add(i);
                else if (j == 1)
                    t.Add(i != LocalPositions.Count - 1 ? LocalPositions.Count + (i + 1) : LocalPositions.Count);
                else if (j == 2)
                    t.Add(LocalPositions.Count + i);
                else if (j == 3)
                    t.Add(i);
                else if (j == 4)
                    t.Add(i != LocalPositions.Count - 1 ? (i + 1) : 0);
                else if (j == 5)
                    t.Add(i != LocalPositions.Count - 1 ? LocalPositions.Count + (i + 1) : LocalPositions.Count);
            }
            tempIntList.AddRange(t);
        }

        // Create the Vector3 vertices
        var vertices = new Vector3[temp.Length * 2];
        Vector2[] tempAllVertices = new Vector2[temp.Length * 2];
        temp.CopyTo(tempAllVertices, 0);
        temp.CopyTo(tempAllVertices, temp.Length);
        for (int i = 0; i < vertices.Length; i++)
        {
            vertices[i] = new Vector3(tempAllVertices[i].x, i < vertices.Length / 2 ? -RadarMeshHeight : RadarMeshHeight, tempAllVertices[i].y);
        }

        // Create the mesh
        Mesh mesh = new Mesh
        {
            vertices = vertices,
            triangles = tempIntList.ToArray()
        };
        mesh.RecalculateNormals();
        mesh.RecalculateBounds();
        GetComponent<MeshFilter>().sharedMesh = mesh;

        var meshCollider = GetComponent<MeshCollider>();
        if (meshCollider == null)
            meshCollider = gameObject.AddComponent<MeshCollider>();
        meshCollider.sharedMesh = mesh;
        meshCollider.convex = true;
    }

    public void ClearMesh()
    {
        var meshFilter = GetComponent<MeshFilter>();
        if (meshFilter != null)
            meshFilter.sharedMesh.Clear();
    }
}

public class Triangulator
{
    private List<Vector2> m_points = new List<Vector2>();

    public Triangulator(Vector2[] points)
    {
        m_points = new List<Vector2>(points);
    }

    public int[] Triangulate()
    {
        List<int> indices = new List<int>();

        int n = m_points.Count;
        if (n < 3)
            return indices.ToArray();

        int[] V = new int[n];
        if (Area() > 0)
        {
            for (int v = 0; v < n; v++)
                V[v] = v;
        }
        else
        {
            for (int v = 0; v < n; v++)
                V[v] = (n - 1) - v;
        }

        int nv = n;
        int count = 2 * nv;
        for (int v = nv - 1; nv > 2;)
        {
            if ((count--) <= 0)
                return indices.ToArray();

            int u = v;
            if (nv <= u)
                u = 0;
            v = u + 1;
            if (nv <= v)
                v = 0;
            int w = v + 1;
            if (nv <= w)
                w = 0;

            if (Snip(u, v, w, nv, V))
            {
                int a, b, c, s, t;
                a = V[u];
                b = V[v];
                c = V[w];
                indices.Add(a);
                indices.Add(b);
                indices.Add(c);
                for (s = v, t = v + 1; t < nv; s++, t++)
                    V[s] = V[t];
                nv--;
                count = 2 * nv;
            }
        }

        indices.Reverse();
        return indices.ToArray();
    }

    private float Area()
    {
        int n = m_points.Count;
        float A = 0.0f;
        for (int p = n - 1, q = 0; q < n; p = q++)
        {
            Vector2 pval = m_points[p];
            Vector2 qval = m_points[q];
            A += pval.x * qval.y - qval.x * pval.y;
        }
        return (A * 0.5f);
    }

    private bool Snip(int u, int v, int w, int n, int[] V)
    {
        int p;
        Vector2 A = m_points[V[u]];
        Vector2 B = m_points[V[v]];
        Vector2 C = m_points[V[w]];
        if (Mathf.Epsilon > (((B.x - A.x) * (C.y - A.y)) - ((B.y - A.y) * (C.x - A.x))))
            return false;
        for (p = 0; p < n; p++)
        {
            if ((p == u) || (p == v) || (p == w))
                continue;
            Vector2 P = m_points[V[p]];
            if (InsideTriangle(A, B, C, P))
                return false;
        }
        return true;
    }

    private bool InsideTriangle(Vector2 A, Vector2 B, Vector2 C, Vector2 P)
    {
        float ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
        float cCROSSap, bCROSScp, aCROSSbp;

        ax = C.x - B.x; ay = C.y - B.y;
        bx = A.x - C.x; by = A.y - C.y;
        cx = B.x - A.x; cy = B.y - A.y;
        apx = P.x - A.x; apy = P.y - A.y;
        bpx = P.x - B.x; bpy = P.y - B.y;
        cpx = P.x - C.x; cpy = P.y - C.y;

        aCROSSbp = ax * bpy - ay * bpx;
        cCROSSap = cx * apy - cy * apx;
        bCROSScp = bx * cpy - by * cpx;

        return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
    }
}